 
 
/**************************************************************************
							Socks V5 Server
							
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

   Coded By Matrix86 ----> matrix86 {AT} tuxmealux {DOT} net
 
****************************************************************************/

#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <signal.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <stdarg.h>
#include <sys/wait.h>
#include <pthread.h>
#include <time.h>
#include "mptcpMgr.h"

#ifndef SIGFUNC_DEFINED
typedef void            (*sigfunc_t)();
#endif

#define BUFFERSIZE 65536

#define PORT 		9001
#define VERSION		"0.1_alpha"
#define AUTHOR		"Matrix86"
#define SITE		"http://www.tuxmealux.net"


#define SOCKS5 		05
#define SOCKS4 		04

#define IPV4   		1
#define IPV6   		4
#define DOMAIN 		3

#define TCPCONN		01
#define TCPBIND		02
#define UDPCONN		03

#define REQ_OK      00
#define REQ_FAIL    01
#define NO_ALLOW    02
#define NET_UNREAC  03
#define HOST_UNREAC 04
#define CON_REFUSED 05
#define TTL_EXPIRED 06
#define ERR_CMD     07
#define ERR_ADDR    08

#define NOAUTH 		00

#define FASE1		1
#define FASE2		2
#define FASE3		3
#define FASE4		4

#define TIMEOUT		30

int dm = 0;

typedef struct context {
	int socket_client;
	int socket_server;
	int handshake;
	struct sockaddr_in addrclient;
	unsigned int version;
	unsigned int type;
	unsigned int DstPort;
	unsigned char DstAddr[1024];
	unsigned char * last_request;
} ss_context;


int ss_fprintf( FILE *stream, const char *fmt, ... ) {
	int c = 0;
  	va_list args;
  	va_start( args, fmt );
  
  	// If daemon mode is off print text
  	if( !dm )
  		c = vfprintf( stream, fmt, args );
  	
  	va_end(args);
  	
  	return c;
}

void die( char *text ) {
	ss_fprintf( stdout, "[ERROR] %s\n", text );
	exit(1);
}

void ss_banner(){
	printf("*****************************************\n");
	printf("*          SIMPLE SOCKS PROXY           *\n");
	printf("*                                       *\n");
	printf("* VERSION: %s                    *\n",VERSION);
	printf("* Author:  %s                     *\n",AUTHOR);
	printf("* Site:    %s     *\n",SITE);
	printf("*                                       *\n");
	printf("*****************************************\n\n");
}

void ss_usage( char * ex ) {
	printf( "Usage : %s <options>\n", ex );
	printf( "\tOptions : \n" );
	printf( "\t\t-p <num>  : set listen port.\n" );
	printf( "\t\t-d        : daemon mode.\n" );
	printf( "\t\t-h        : show this help.\n\n" );
}

void set_timeout( int sd ){
	struct timeval tv;
	tv.tv_sec 	= TIMEOUT;
  	tv.tv_usec 	= 0;
  	
	mptcp_setsockopt( sd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(struct timeval) );
	mptcp_setsockopt( sd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(struct timeval) );
}

void set_signal( int sig, sigfunc_t handler ) {
	struct sigaction n, o;

	memset(&n, 0, sizeof n);
	n.sa_handler = handler;

	if (sig != SIGALRM) {
		n.sa_flags = SA_RESTART;
	}

	if (sigaction(sig, &n, &o) < 0)
		return SIG_ERR;
	return o.sa_handler;
}

int ss_send( ss_context * ctx, unsigned char * buffer, int lenght ) {
	if( mptcp_send( ctx->socket_client, buffer, lenght, 0 ) < 0 ) {
		return 1;
	}
	
	return 0;
}

int ss_connection( ss_context * ctx ) {
	struct sockaddr_in server;
	struct hostent * data;
	char ip[INET6_ADDRSTRLEN];
	
	memset( (void *) ip, 0, sizeof(ip) );

	if ( ( ctx->socket_server = mptcp_socket( AF_INET, SOCK_STREAM, 0, htons(PORT) ) ) < 0 )
	{
	ss_fprintf( stderr, "[WARNING] Failed to create socket.\n" );
	return -1;
	}

	memset( (void *) &server, 0, sizeof(struct sockaddr_in) );
	server.sin_family = AF_INET;
	server.sin_port = htons( ctx->DstPort );
	
	// If a domain, get his IP address
	if( ctx->type == DOMAIN ) {
		if ( ( data = gethostbyname( (const char *) ctx->DstAddr ) ) == NULL ) {
        	ss_fprintf( stderr, "[WARNING] gethostbyname %s.\n", strerror(errno) );
			return -1;
    	}
    	inet_ntop(data->h_addrtype, data->h_addr, ip, sizeof(ip));
    	ss_fprintf( stdout, "- Destination IP: %s\n", ip );
	}
	else if( ctx->type == IPV4 )
		strcpy( ip, (const char *) ctx->DstAddr );
	
	server.sin_addr.s_addr = inet_addr( ip );
	
	if( mptcp_connect ( ctx->socket_server, (struct sockaddr *) &server, sizeof( struct sockaddr ) ) < 0 )
	{
		ss_fprintf( stderr, "[WARNING] connect %s.\n", strerror(errno) );
		return -1;
	}
	//printf("Connected to %s\n", ip);
	set_timeout( ctx->socket_server );
	
	return 0;
}

int ss_pkt_parse( ss_context * ctx ) {
	int nmethods, i, num;
	unsigned char buff[BUFFERSIZE];
	
	if( (ctx->handshake == FASE1) || (ctx->handshake == FASE2) ) {
		switch( ctx->last_request[0] ) {
			case SOCKS4 :
				// Ignore this version
				return -1;
				break;
			case SOCKS5:
				ctx->version = SOCKS5;
				//printf( "SOCKS5\n" );
				break;
			default:
				//fprintf( stdout, "Richiesta non socks5\n" );
				return -1;
		}
	}
	
	// First phase of handshake
	if( ctx->handshake == FASE1 ) {
		//printf("Fase1\n");
		// numbers of methods supported
		nmethods = ctx->last_request[1];
		
		// Check no authentication methods into client methods list
		for( i = 0; ( i < nmethods ) && ( i < BUFFERSIZE - 2 ); i++ ){
			if( ctx->last_request[i+2] == NOAUTH ) {
				buff[0] = SOCKS5;
				buff[1] = NOAUTH;
				if( ss_send( ctx, buff, 2 ) ) return -1;
				ctx->handshake = FASE2;
				return 0;
			}
		}
		return -1;
	}
	
	// Second phase of handshake
	if( ctx->handshake == FASE2 ) {
		//printf("Fase2\n");
		unsigned int i, s, len, DstPort = 0;
		unsigned char DstAddr[1024];
	
		if( ctx->last_request[1] != TCPCONN ) return -1;
		if( ctx->last_request[2] != 00 ) return -1;
		
		switch( ctx->last_request[3] ) {
			case IPV4:
				//printf("IPv4\n");
				ctx->type = IPV4;
				len = 10;
				DstPort += ctx->last_request[8];
				DstPort <<= 8;
				DstPort += ctx->last_request[9];
				snprintf( (char *) DstAddr, sizeof(DstAddr), "%hu.%hu.%hu.%hu",		(unsigned char)ctx->last_request[4],
																   					(unsigned char)ctx->last_request[5],
																   					(unsigned char)ctx->last_request[6],
																   					(unsigned char)ctx->last_request[7] 
																   					);
				strcpy( (char *) ctx->DstAddr, (const char *) DstAddr );
				ctx->DstPort = DstPort;
				
				ss_fprintf( stdout, "[%s] Received request for %s:%d.\n", inet_ntoa( ctx->addrclient.sin_addr ), DstAddr, DstPort );
				
				// Trying to connect
				if( ss_connection( ctx ) < 0 ) {
					// Connection error
					buff[0] = SOCKS5;
					buff[1] = REQ_FAIL;
					buff[2] = 00;
				}
				else {
					//Connection done
					buff[0] = SOCKS5;
					buff[1] = REQ_OK;
					buff[2] = 00;
				}
				
				for( i = 3; i <= 7 + 2; i++ )
					buff[i] = ctx->last_request[i];
				
				if( ss_send( ctx, buff, len ) ) {
					ss_fprintf( stderr, "ss_send problem.\n" );
					return -1;
				}
				else {
					if( buff[1] == REQ_FAIL ) return -1;
					ctx->handshake = FASE3;
					return 0;
				}
				break;
			case DOMAIN:
				//printf("DOMAIN");
				ctx->type = DOMAIN;
				num = ctx->last_request[4]+5;
				len = num+2;
				DstPort += ctx->last_request[num];
				DstPort <<= 8;
				DstPort += ctx->last_request[num + 1];
				
				for( i = 0, s = 5; s < num; s++, i++ ) {
					if( i >= 64 ) break;
					DstAddr[i] = ctx->last_request[s];
				}
				DstAddr[i] = '\0';
				
				strcpy( (char *) ctx->DstAddr, (const char *) DstAddr );
				ctx->DstPort = DstPort;
				
				ss_fprintf( stdout, "[%s] Received request for %s:%d.\n", inet_ntoa( ctx->addrclient.sin_addr ), DstAddr, DstPort );
				
				// Trying to connect
				if( ss_connection( ctx ) < 0 ) {
					// Connection error
					buff[0] = SOCKS5;
					buff[1] = REQ_FAIL;
					buff[2] = 00;
				}
				else {
					// Connection done
					buff[0] = SOCKS5;
					buff[1] = REQ_OK;
					buff[2] = 00;
				}

				for( i = 3; i <= 5 + ctx->last_request[4] + 2; i++ )
					buff[i] = ctx->last_request[i];
					
				if( ss_send( ctx, buff, len ) ) {
					ss_fprintf( stderr, "ss_send problem\n" );
					return -1;
				}
				else {
					if( buff[1] == REQ_FAIL ) return -1;
					ctx->handshake = FASE3;
					return 0;
				}
				break;
			case IPV6:
				//strcpy( type, "IPV6 (no Supported)." );
				return -1;
				break;
			default:
				return -1;
				break;
		}
		
		
		
	}
	
	return -1;
}

void timeout(int signo)
{
  return;
}

void ss_pipe( ss_context * ctx ) {
	fd_set 			rfds;
	int      		nfds, sfd;
	struct   timeval tv;
	int 			mslen = 0;
	unsigned char * buffer;
	
	buffer = ( unsigned char * ) calloc( sizeof(char) * BUFFERSIZE, 1 );
	
	set_timeout( ctx->socket_client );
	set_timeout( ctx->socket_server );
	
	nfds = ( ctx->socket_server > ctx->socket_client ? ctx->socket_server : ctx->socket_client );
	set_signal( SIGALRM, timeout );
	for (;;) {
		FD_ZERO( &rfds );
		FD_SET( ctx->socket_client, &rfds );
		FD_SET( ctx->socket_server, &rfds );
		
		tv.tv_sec = 60; tv.tv_usec = 0;
		
		sfd = mptcp_select( nfds+1, &rfds, NULL, NULL, &tv);
		if( sfd > 0 ) {
			if( FD_ISSET( ctx->socket_server, &rfds ) ) {
				// Forward from server to client
				if( ( mslen = read( ctx->socket_server, buffer, BUFFERSIZE ) ) > 0 ) {
					mptcp_send( ctx->socket_client, buffer, mslen, 0 );
				}
				if( mslen == 0 ) break;
				//printf( "Forward from server to client\n" );
				FD_CLR( ctx->socket_server, &rfds );
			}
		  	
		  	if ( FD_ISSET( ctx->socket_client, &rfds ) ) {
				// Forward from client to server
				if( ( mslen = read( ctx->socket_client, buffer, BUFFERSIZE ) ) > 0 ) {
					mptcp_send( ctx->socket_server, buffer, mslen, 0 );
				}
				if( mslen == 0 ) break;
				//printf( "Forward from client to server\n" );
				FD_CLR( ctx->socket_client, &rfds );
		  	}
		  	
		} else if( sfd < 0 ) {
			if( errno != EINTR ) break;
		}
	}
	
	return;
}

void * ss_manage( void * context ) {
	ss_context * ctx = ( ss_context * )context;
	int mslen = 0;
	unsigned char * buffer;
	buffer = ( unsigned char * ) calloc( sizeof(char)*BUFFERSIZE, 1 );
	ctx->handshake = FASE1;
	
	while( ( mslen = read( ctx->socket_client, buffer, BUFFERSIZE ) ) > 0 || errno == EAGAIN ) {
		ctx->last_request = buffer;
		if( ss_pkt_parse( ctx ) < 0 ) {
			break;
		}
		if( ctx->handshake == FASE3 ) {
			ss_pipe( ctx );
			break;
		}
	}
	
	//printf("Exit\n");
	mptcp_close( ctx->socket_server );
	mptcp_close( ctx->socket_client );
	free( buffer );
	pthread_exit(NULL);
}

int main( int argc, char *argv[] ) {
	struct sockaddr_in server;
	struct sockaddr_in client;
	int mainSocket, subSocket, c;
	int port = PORT;
	fd_set readfds;
        int conn, i,max_fd;
	int fd[10];
	struct timeval tv;	
	ss_banner();
	tv.tv_sec = 0;
	tv.tv_usec = 0;	
	while( ( c = getopt( argc, argv, "dhp:" ) ) != -1 ){
		switch(c){
			case 'd' : dm   = 1; break;
			case 'p' : port = atoi(optarg); break;
			case 'h':
			default  : 
					   ss_usage( strdup( argv[0] ) );
					   return -1;
		}
	}
	
	if( dm ) daemon(1,0);

	printf("PORT : %d\n", port);
	
	if( ( mainSocket = mptcp_socket(PF_INET, SOCK_STREAM, 0, port )) < 0 ) {
		die( strerror(errno) );
	}
	
	set_timeout( mainSocket );
	
	memset( (void *)&server, 0, sizeof(server) );
	
	server.sin_family 		= PF_INET;
	server.sin_port 		= htons( port );
	server.sin_addr.s_addr 	= 0;
	
	/* Binding */
	if( mptcp_bind( mainSocket, (struct sockaddr *)&server, sizeof(struct sockaddr_in), argv, argc, 1) < 0){
		die( strerror(errno) );
	}
	
	if ( mptcp_listen( mainSocket, 5 ) < 0 ) {
		die( strerror(errno) );
	}
	
	ss_fprintf( stdout, "[@] Listening on port %d.\n", port );
	ss_fprintf( stdout, "[@] Waiting for connection.\n" );
	fflush(stdout);

 	conn = -1;	
	while(1) {
      my_upcall_worker(NULL);

	FD_ZERO(&readfds);
      	FD_SET(mainSocket,&readfds);
      	max_fd = mainSocket;

	for (i=0;i<=conn;i++){
        FD_SET(fd[i],&readfds);

        if (fd[i]>max_fd)
          max_fd = fd[i];
      }

      tv.tv_sec = 0;
      tv.tv_usec = 0;

      mptcp_select(max_fd+1,&readfds, NULL, NULL, &tv);


      if (FD_ISSET(mainSocket,&readfds)){
		socklen_t addrlen = sizeof( struct sockaddr_in );
		subSocket = mptcp_accept(mainSocket, (struct sockaddr *)&client, &addrlen );
		printf("subSocket is %d\n", subSocket);
		//while( ( ( subSocket = mptcp_accept( mainSocket, (struct sockaddr *)&client, &addrlen ) ) < 0 ) && ( errno == EINTR ) ) ;
		
		if( subSocket > 0 ) {
		printf("Socket Read!!\n");
			int on = 1;
			mptcp_setsockopt( subSocket, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on) );
			
			pthread_t 	thread;
			
			ss_context *context;
			context = (ss_context *) calloc( sizeof(ss_context), 1 );
			context->socket_client = subSocket;
			context->addrclient    = client;
			
			ss_fprintf( stdout, "[@] Received connection from: %s.\n", inet_ntoa( client.sin_addr ) );
			fflush(stdout);
			
			pthread_create( &thread, 0, ss_manage, (void *) context );
		}
	}
}
}

